import time
from PySide.QtGui import *

class MyWindow(QWidget):
    def __init__(self):
        super(MyWindow, self).__init__()

        self.btnRestart = QPushButton("Reset")
        self.btnRestart.clicked.connect(self.home_position)

        self.btnStartPos = QPushButton("Start Pos")
        self.btnStartPos.clicked.connect(self.move_to_start)

        self.btnToggleWifi = QPushButton("Show / Hide Wifi")
        self.btnToggleWifi.clicked.connect(self.toggle_wifi)

        self.btnHidePins = QPushButton("Hide Pins")
        self.btnHidePins.clicked.connect(self.hide_pins)

        self.btnShowPins = QPushButton("Show Pins")
        self.btnShowPins.clicked.connect(self.show_pins)

        self.btnStack = QPushButton("Stack Pico and backpack")
        self.btnStack.clicked.connect(self.stack)

        layout = QVBoxLayout()

        layout.addWidget(self.btnRestart)
        layout.addWidget(self.btnStartPos)
        layout.addWidget(self.btnToggleWifi)
        layout.addWidget(self.btnHidePins)
        layout.addWidget(self.btnShowPins)
        layout.addWidget(self.btnStack)

        self.setLayout(layout)
        self.setWindowFlags(Qt.WindowStaysOnTopHint)
        self.show()

    def home_position(self):
        FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
            "Body"
        ).Placement = App.Placement(
            App.Vector(0, 0, 0), App.Rotation(App.Vector(0, 0, 1), 0)
        )
        FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
            "Body001"
        ).Placement = App.Placement(
            App.Vector(0, 0, -7), App.Rotation(App.Vector(0, 0, 1), 0)
        )
        Gui.updateGui()

    def move_to_start(self):
        # Move to start position
        # Move Pico to one side
        FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
            "Body"
        ).Placement = App.Placement(
            App.Vector(32, 0, -1), App.Rotation(App.Vector(0, 0, 1), 0)
        )
        # Rotate backpack
        # Move backpack to same level as Pico
        FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
            "Body001"
        ).Placement = App.Placement(
            App.Vector(0, 0, 1), App.Rotation(App.Vector(0, 1, 0), 180)
        )

        self.camera_start()

    def stack(self):
        self.move_backpack()
        self.move_pico()

    def move_backpack(self):
        # Move backpack up
        for zcount in range(10, 100):
            zpos = zcount / 10
            FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
                "Body001"
            ).Placement = App.Placement(
                App.Vector(0, 0, zpos), App.Rotation(App.Vector(0, 1, 0), 180)
            )
            Gui.updateGui()
            time.sleep(0.01)
        # Rotate backpack
        for rot_pos in range (180, 0, -1):
            FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
            "Body001"
        ).Placement = App.Placement(
            App.Vector(0, 0, zpos), App.Rotation(App.Vector(0, 1, 0), rot_pos)
        )
            Gui.updateGui()
            time.sleep(0.01)
        # Move backpack down
        for zcount in range(100, 0, -1):
            zpos = zcount / 10
            FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
                "Body001"
            ).Placement = App.Placement(
                App.Vector(0, 0, zpos), App.Rotation(App.Vector(0, 1, 0), 0)
            )
            Gui.updateGui()
            time.sleep(0.01)


    def move_pico(self):
        # Move Pico up
        for zcount in range (10, 100):
            zpos = zcount / 10
            FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
                "Body"
            ).Placement = App.Placement(
                App.Vector(32, 0, zpos), App.Rotation(App.Vector(0, 0, 1), 0)
            )
            Gui.updateGui()
            time.sleep(0.01)

        # move pico across
        for xpos in range (32, -1, -1):
            FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
                "Body"
            ).Placement = App.Placement(
                App.Vector(xpos, 0, zpos), App.Rotation(App.Vector(0, 0, 1), 0)
            )
            Gui.updateGui()
            time.sleep(0.01)

        # move pico down
        for zcount in range (100, 45, -1):
            zpos = zcount / 10
            FreeCAD.getDocument("rpi_pico_model_1_6").getObject(
                "Body"
            ).Placement = App.Placement(
                App.Vector(xpos, 0, zpos), App.Rotation(App.Vector(0, 0, 1), 0)
            )
            Gui.updateGui()
            time.sleep(0.01)

    def toggle_wifi(self):
        Gui.Selection.addSelection("rpi_pico_model_1_6", "Body001")
        Gui.runCommand("Std_ToggleVisibility", 0)
        Gui.Selection.clearSelection()

    def show_pins(self):
        Gui.Selection.addSelection("rpi_pico_model_1_6", "Body", "Pad013.")
        Gui.runCommand("Std_HideSelection", 0)
        Gui.Selection.addSelection("rpi_pico_model_1_6", "Body", "LinearPattern005.")
        Gui.runCommand("Std_ShowSelection", 0)
        Gui.Selection.clearSelection()

    def hide_pins(self):
        Gui.Selection.addSelection("rpi_pico_model_1_6", "Body", "LinearPattern005.")
        Gui.runCommand("Std_HideSelection", 0)
        Gui.Selection.addSelection("rpi_pico_model_1_6", "Body", "Pad013.")
        Gui.runCommand("Std_ShowSelection", 0)
        Gui.Selection.clearSelection()


    def camera_start(self):
        self.RotateView (1,0,0, 10)


    #RotateView from https://wiki.freecadweb.org/Macro_Rotate_View_Free
    #RotateView(0,1,0,45)
    def RotateView(self, axisX=1.0,axisY=0.0,axisZ=0.0,angle=45.0):
        import math
        from pivy import coin
        try:
            cam = Gui.ActiveDocument.ActiveView.getCameraNode()
            rot = coin.SbRotation()
            rot.setValue(coin.SbVec3f(axisX,axisY,axisZ),math.radians(angle))
            nrot = cam.orientation.getValue() * rot
            cam.orientation = nrot
            print( axisX," ",axisY," ",axisZ," ",angle)
        except Exception:
            print( "Not ActiveView ")


app = MyWindow()